#!/usr/bin/env python3
# coding: utf8

from numpy import array, deg2rad
from math import sin, cos

from robotics.io.pcd import load_point_cloud
from robotics.registration.util import transform_point_cloud, explain_quaternion
from robotics.registration.icp import Aligner
from robotics.registration.correspondence.oneway import find_correspondences
from robotics.registration.rotation.eigen import extract_rotation
from robotics.registration.criteria.rms import stop_condition


source_cloud = load_point_cloud('/Users/yfwz100/ws/siasun/data/20160111/p%d.pcd' % 1) / 10
target_cloud = load_point_cloud('/Users/yfwz100/ws/siasun/data/20160111/p%d.pcd' % 2) / 10

a = Aligner(extract_rotation, find_correspondences, stop_condition(max_iterations=50, max_delta_err=1, min_err=1),
            precision=0)
r, t = a.align(source_cloud, target_cloud)

print('Rotation:\n%s' % r)
print('Quaternion:\n%s,%s' % explain_quaternion(r))
print('Translation:\n%s' % t)
